#include "zdt_usart.h"
#include "cmsis_os.h"
#include "general_def.h"

#define BUFFERSIZE 50u

#define ZDT_ISNEGATIVE(num) (num==0)?(1):(-1)

#define ZDT_MAX_NUM 4
#define CIRCLE_COUNT 65536

#define ABS(x) ((x) < 0 ? -(x) : (x))

static uint8_t databuf[BUFFERSIZE];
static uint8_t idx;
static ZDTInstance *zdtinstance[ZDT_MAX_NUM];
volatile uint16_t receive_size=20;

static void ZDT_USARTSendCmd(ZDTInstance *zdt, uint8_t cmd[], uint8_t size)
{
	USARTSend(zdt->motor_usart_instance, cmd, size, USART_TRANSFER_DMA);
	osDelay(1);
}

void ZDT_Read_Sys_Params(ZDTInstance *zdt, SysParams_t s)
{
	volatile uint8_t i = 0;
	uint8_t cmd[16] = { 0 };

	cmd[i] = zdt->id;
	++i;                 

	switch (s)

	{
	case S_VER:
		cmd[i] = 0x1F;	// 读取固件版本和对应的硬件版本
		++i;
		break;
	case S_RL:
		cmd[i] = 0x20;  // 读取相电阻和相电感
		++i;
		break;
	case S_PID:
		cmd[i] = 0x21;  // 读取位置环PID参数
		++i;
		break;
	case S_VBUS:
		cmd[i] = 0x24;  // 读取总线电压
		++i;
		break;
	case S_CPHA:
		cmd[i] = 0x27;  // 读取相电流 
		++i;
		break;
	case S_ENCL:
		cmd[i] = 0x31; // 读取经过线性化校准后的编码器值
		++i;
		receive_size=5;
		break;
	case S_TPOS:
		cmd[i] = 0x33;  // 读取电机目标位置
		++i;
		break;
	case S_VEL:
		cmd[i] = 0x35;  // 读取电机实时转速
		++i;
		receive_size=6;
		break;
	case S_CPOS:
		cmd[i] = 0x36;  // 读取电机实时位置
		++i;
		receive_size=8;
		break;
	case S_PERR:
		cmd[i] = 0x37;  // 读取电机位置误差
		++i;
		break;
	case S_FLAG:
		cmd[i] = 0x3A;  // 读取电机状态标志位
		++i;
		break;
	case S_ORG:
		cmd[i] = 0x3B;
		++i;
		break;
	case S_Conf:
		cmd[i] = 0x42;  // 读取驱动配置参数
		++i;
		cmd[i] = 0x6C;
		++i;
		break;
	case S_State:
		cmd[i] = 0x43; // 读取系统状态参数
		++i;
		cmd[i] = 0x7A;
		++i;
		break;
	case S_PULSE:
		cmd[i] = 0x32;  // 读取输入脉冲数
		++i;
		break;
	default:
		break;
	}

	cmd[i] = 0x6B;
	++i;

	ZDT_USARTSendCmd(zdt, cmd, i);
}



void ZDT_En_Control(ZDTInstance *zdt, bool state, bool snF)
{
	uint8_t cmd[16] = { 0 };

	cmd[0] = zdt->id;                     
	cmd[1] = 0xF3;                     
	cmd[2] = 0xAB;                       
	cmd[3] = (uint8_t) state;             
	cmd[4] = snF;                        
	cmd[5] = 0x6B;                     

	ZDT_USARTSendCmd(zdt, cmd, 6);
}

static void ZDT_Vel_Control(ZDTInstance *zdt, uint8_t dir, uint16_t vel, uint8_t acc, bool snF)
{
	uint8_t cmd[16] = { 0 };


	cmd[0] = zdt->id;                
	cmd[1] = 0xF6;                  
	cmd[2] = dir;                      
	cmd[3] = (uint8_t) (vel >> 8);      
	cmd[4] = (uint8_t) (vel >> 0);     
	cmd[5] = acc;                        
	cmd[6] = snF;                        
	cmd[7] = 0x6B;  

	ZDT_USARTSendCmd(zdt, cmd, 8);
}

void ZDT_Vel_Send(ZDTInstance *zdt, int16_t vel, uint8_t acc)
{
	uint8_t dir;
	uint16_t vvel;
	dir = (vel > 0) ? CCW : CW;
	vel = ABS(vel);
	ZDT_Vel_Control(zdt, dir, vel, acc, false);
}

void ZDT_Reset_CurPos_To_Zero(ZDTInstance *zdt)
{
	uint8_t cmd[16] = { 0 };

	
	cmd[0] = zdt->id;             
	cmd[1] = 0x0A;                   
	cmd[2] = 0x6D;                    
	cmd[3] = 0x6B;                     

	ZDT_USARTSendCmd(zdt, cmd, 4);
}

void ZDT_Pos_Control(ZDTInstance *zdt, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF)
{
	uint8_t cmd[16] = { 0 };


	cmd[0] = zdt->id;                    
	cmd[1] = 0xFD;                      
	cmd[2] = dir;                       
	cmd[3] = (uint8_t) (vel >> 8);      
	cmd[4] = (uint8_t) (vel >> 0);      
	cmd[5] = acc;                       
	cmd[6] = (uint8_t) (clk >> 24);      
	cmd[7] = (uint8_t) (clk >> 16);    
	cmd[8] = (uint8_t) (clk >> 8);     
	cmd[9] = (uint8_t) (clk >> 0);      
	cmd[10] = raF;                      
	cmd[11] = snF;                       
	cmd[12] = 0x6B;                     


	ZDT_USARTSendCmd(zdt, cmd, 13);
}

ZDTReturn_t ZDT_UnPack(uint8_t data[])
{
    ZDTInstance *motor=NULL;
	uint8_t id;
	ZDTCommand_t command;
	memcpy(databuf, data, 16);
	id = databuf[0];
	command = databuf[1];

    for(int i =0;i<ZDT_MAX_NUM;i++)
    {
        if(zdtinstance[i]->id == id)
        {
            motor=zdtinstance[i];
        }
    }
    if (motor == NULL)
        return ZDT_ID_ERROR;

    motor->received_flag=1;

	switch (command)
	{

	case ZDT_PULSE:
        motor->state.Pulse =
		ZDT_ISNEGATIVE(databuf[2]) * ((databuf[3] << 24) | (databuf[4] << 16) | (databuf[5] << 8) | (databuf[6]));
		break;
	case ZDT_VEL:
        motor->state.CurrentSpeed = (int16_t) ((databuf[3] << 8) | (databuf[4]))*((databuf[2]==1)?-1:1);
		break;

	case ZDT_CPOS:
        motor->state.CurrentPosition = ((databuf[3] << 24)|(databuf[4] << 16)|(databuf[5] << 8) | (databuf[6]))*((databuf[2]==1)?-1:1);

		break;
	case ZDT_ERROR:
		if (databuf[2] == 0xee)
			return ZDT_COMMAND_ERROR;
	default:
		return ZDT_COMMAND_ERROR;
	}
	return ZDT_OK;
}

void ZDT_Angle_Calc(ZDTInstance *zdt, int x)
{
	int quotient;
	float remainder;
	zdt->state.CurrentPosition *= x;
	quotient = zdt->state.CurrentPosition / CIRCLE_COUNT;
	remainder = zdt->state.CurrentPosition % CIRCLE_COUNT;
	zdt->state.last_angle = zdt->state.angle;
	zdt->state.last_rad = zdt->state.rad;
	zdt->state.angle = quotient * 360.0f + (remainder / CIRCLE_COUNT) * 360.0f;
	zdt->state.rad = zdt->state.angle * DEGREE_2_RAD;

	zdt->state.angle = zdt->state.angle * 0.1 + zdt->state.last_angle * 0.9;
	zdt->state.rad = zdt->state.rad * 0.9 + zdt->state.last_rad * 0.1;
}

static void ZDTMotorControlRxCallback()
{
	ZDT_UnPack(zdtinstance[0]->motor_usart_instance->recv_buff);
}

static void ZDTMotorUSARTLostCallback()
{

}

ZDTInstance *ZDTMotorUSARTInit(UART_HandleTypeDef *zdt_usart_handle, ZDT_Motor_Init_Config_s config)
{
	ZDTInstance *motor = (ZDTInstance *)malloc(sizeof(ZDTInstance));
	memset(motor, 0, sizeof(ZDTInstance));
	USART_Init_Config_s conf;
	conf.module_callback = ZDTMotorControlRxCallback;
	conf.recv_buff_size = 16;
	conf.usart_handle = zdt_usart_handle;

	motor->id = config.id;
	motor->motor_usart_instance = USARTRegister(&conf);

	Daemon_Init_Config_s daemon_conf = {
        .callback = ZDTMotorUSARTLostCallback,
        .owner_id = motor,
        .reload_count = 10,
    };
    // motor->motor_daemon = DaemonRegister(&daemon_conf);

    zdtinstance[idx++]=motor;
    return motor;

}